using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
using System.Diagnostics;

namespace XNASubdivision
{
    public class CatmullClarkSubdivider
    {
        QuadMesh m_origQuadMesh;

        internal QuadMesh GetSubdividedMesh(QuadMesh p_quadMesh)
        {
            m_origQuadMesh = p_quadMesh;
            DetermineOriginalMeshEdges();

            QuadMesh quadmesh = new QuadMesh();
            
            // I.1. create Vertices
            //create face point, edge points and 4 quads from each quad
            int nOldVertices = m_origQuadMesh.Vertices.Length;
            int nOldQuads = m_origQuadMesh.Quads.Length;
            int nOldEdges = m_origQuadMesh.Edges.Count;

            int nVertices = nOldVertices + nOldQuads + nOldEdges;
            quadmesh.Vertices = new Vector3[nVertices];
            quadmesh.Normals = new Vector3[nVertices];
            quadmesh.TextureUVs = new Vector2[nVertices];
            int iVertex = 0;
            for (int i = 0; i < nOldVertices; i++)
            {
                quadmesh.Vertices[iVertex] = m_origQuadMesh.Vertices[i];
                quadmesh.Normals[iVertex] = m_origQuadMesh.Normals[i];
                quadmesh.TextureUVs[iVertex] = m_origQuadMesh.TextureUVs[i];
                iVertex++;
            }

            for (int i = 0; i < nOldQuads; i++)
            {
                Vertex vertex = m_origQuadMesh.CreateQuadCenter(i);
                quadmesh.Vertices[iVertex] = vertex.pos;
                quadmesh.Normals[iVertex] = vertex.normal;
                quadmesh.TextureUVs[iVertex] = vertex.textureUV;
                iVertex++;
            }

            for (int i = 0; i < nOldEdges; i++)
            {
                Vertex vertex = m_origQuadMesh.CreateEdgeMiddle(i);
                quadmesh.Vertices[iVertex] = vertex.pos;
                quadmesh.Normals[iVertex] = vertex.normal;
                quadmesh.TextureUVs[iVertex] = vertex.textureUV;
                iVertex++;
            }

            // I.2. create new quads
            int nQuads = 4 * m_origQuadMesh.Quads.Length;
            quadmesh.Quads = new Quad[nQuads];
            for (int i = 0; i < nOldQuads; i++)
            {
                int centerIndex = nOldVertices + i;
                int e0half = nOldVertices + nOldQuads + m_origQuadMesh.Quads[i].edgeInd0;
                int e1half = nOldVertices + nOldQuads + m_origQuadMesh.Quads[i].edgeInd1;
                int e2half = nOldVertices + nOldQuads + m_origQuadMesh.Quads[i].edgeInd2;
                int e3half = nOldVertices + nOldQuads + m_origQuadMesh.Quads[i].edgeInd3;

                quadmesh.Quads[4 * i] = new Quad(m_origQuadMesh.Quads[i].vertInd0, e0half, centerIndex, e3half);
                quadmesh.Quads[4 * i + 1] = new Quad(m_origQuadMesh.Quads[i].vertInd1, e1half, centerIndex, e0half);
                quadmesh.Quads[4 * i + 2] = new Quad(m_origQuadMesh.Quads[i].vertInd2, e2half, centerIndex, e1half);
                quadmesh.Quads[4 * i + 3] = new Quad(m_origQuadMesh.Quads[i].vertInd3, e3half, centerIndex, e2half);
            }


            // II. modify the edge points
            // http://www.holmes3d.net/graphics/subdivision/
            //An edge point is created for each old edge, defined as the average of the midpoint of the original
            //edge and the midpoint of the two new face points for the polygons that adjoin the original edge.
            for (int i = 0; i < nOldEdges; i++)
            {
                int edgeStart = m_origQuadMesh.Edges[i].vertInd0;
                int edgeEnd = m_origQuadMesh.Edges[i].vertInd1;

                List<int> quadIndices = m_origQuadMesh.FindQuadsForEdge(i);
                Debug.Assert(quadIndices.Count == 1 || quadIndices.Count == 2);
                if (quadIndices.Count == 1)
                {
                    int quad0Center = nOldVertices + quadIndices[0];
                    int halfEdgeIndex = nOldVertices + nOldQuads + i;
                    
                    Vertex vertex = quadmesh.CreateAverage(edgeStart, edgeEnd, quad0Center);

                    quadmesh.Vertices[halfEdgeIndex] = vertex.pos;
                    quadmesh.Normals[halfEdgeIndex] = vertex.normal;
                    quadmesh.TextureUVs[halfEdgeIndex] = vertex.textureUV;

                } else if (quadIndices.Count == 2)
                {
                    int quad0Center = nOldVertices + quadIndices[0];
                    int quad1Center = nOldVertices + quadIndices[1];
                    int halfEdgeIndex = nOldVertices + nOldQuads + i;
                    Vertex vertex = quadmesh.CreateAverage(edgeStart, edgeEnd, quad0Center, quad1Center);
                    quadmesh.Vertices[halfEdgeIndex] = vertex.pos;
                    quadmesh.Normals[halfEdgeIndex] = vertex.normal;
                    quadmesh.TextureUVs[halfEdgeIndex] = vertex.textureUV;
                }
            }

            // III. modify original vertices
            //http://symbolcraft.com/graphics/subdivision/
            for (int i = 0; i < nOldVertices; i++)
            {
                List<int> QList = m_origQuadMesh.FindQuadsForVertex(i);
                List<int> QListVertices = new List<int>(QList);
                for (int j = 0; j < QListVertices.Count; j++)
                {
                    QListVertices[j] += nOldVertices;
                }
                Vertex Q = Average(quadmesh, QListVertices);

                List<int> egdesList = m_origQuadMesh.EdgesForVertex(i);
                List<int> RListVertices = new List<int>(egdesList);
                for (int j = 0; j < RListVertices.Count; j++)
                {
                    RListVertices[j] += nOldVertices + nOldQuads;
                }
                Vertex R = Average(quadmesh, RListVertices);
                int n = egdesList.Count;

                Vertex S;
                S.pos = quadmesh.Vertices[i];
                S.normal = quadmesh.Normals[i];
                S.textureUV = quadmesh.TextureUVs[i];

                Vertex newVertex = CalculateVertNewPos(Q, R, S, n);

                quadmesh.Vertices[i] = newVertex.pos;
                quadmesh.Normals[i] = newVertex.normal;
                quadmesh.TextureUVs[i] = newVertex.textureUV;
            }


            return quadmesh;
        }

        Vertex CalculateVertNewPos(Vertex p_Q, Vertex p_R, Vertex p_S, int p_n)
        {
            Vertex vertex = new Vertex();
            vertex.pos = (p_Q.pos + 2 * p_R.pos + (p_n - 3) * p_S.pos) / (float)p_n;
            vertex.normal = (p_Q.normal + 2 * p_R.normal + (p_n - 3) * p_S.normal);
            vertex.normal.Normalize();
            vertex.textureUV = (p_Q.textureUV + 2 * p_R.textureUV + (p_n - 3) * p_S.textureUV) / (float)p_n;

            return vertex;
        }

        Vertex Average(QuadMesh p_quadmesh, List<int> p_vertices)
        {
            Vertex vertex =  new Vertex();

            foreach (int vertIndex in p_vertices)
            {
                vertex.pos += p_quadmesh.Vertices[vertIndex];
                vertex.normal += p_quadmesh.Normals[vertIndex];
                vertex.textureUV += p_quadmesh.TextureUVs[vertIndex];
            }

            vertex.pos /= (float)p_vertices.Count;
            vertex.normal.Normalize();
            vertex.textureUV /= (float)p_vertices.Count;

            return vertex;
        }

        private void DetermineOriginalMeshEdges()
        {
            for (int i = 0; i < m_origQuadMesh.Quads.Length; i++)
            {
                m_origQuadMesh.Quads[i].edgeInd0 = FindOrCreateEdge(
                            m_origQuadMesh.Quads[i].vertInd0, m_origQuadMesh.Quads[i].vertInd1);
                m_origQuadMesh.Quads[i].edgeInd1 = FindOrCreateEdge(
                            m_origQuadMesh.Quads[i].vertInd1, m_origQuadMesh.Quads[i].vertInd2);
                m_origQuadMesh.Quads[i].edgeInd2 = FindOrCreateEdge(
                            m_origQuadMesh.Quads[i].vertInd2, m_origQuadMesh.Quads[i].vertInd3);
                m_origQuadMesh.Quads[i].edgeInd3 = FindOrCreateEdge(
                            m_origQuadMesh.Quads[i].vertInd3, m_origQuadMesh.Quads[i].vertInd0);
            }
        }

        private int FindOrCreateEdge(int p_vert0, int p_vert1)
        {
            int findIndex = m_origQuadMesh.Edges.IndexOf(new Edge(p_vert0, p_vert1));
            if (findIndex != -1)
                return findIndex;

            findIndex = m_origQuadMesh.Edges.IndexOf(new Edge(p_vert1, p_vert0));
            if (findIndex != -1)
                return findIndex;

            m_origQuadMesh.Edges.Add(new Edge(p_vert0, p_vert1));

            return m_origQuadMesh.Edges.Count - 1;
        }
    }
}
